#include <cmath>
#include "BSplineCurve.h"

using namespace std;

#define DEFAULT_SELECT_INDEX -1
#define DEFAULT_NUM_CP 10
#define DEFAULT_ORDER 1
#define MAX_DELETE_PROXIMITY 5
#define MAX_SELECT_PROXIMITY 8

//========================================================
//                 Constructor/Destructor
//========================================================

/**
    Creates a new instance of BSplineCurve.
*/
BSplineCurve::BSplineCurve()
{
	initializeAttributes();
	updateKnotSequence();
}

/**
    Cleans up memory used by this class.
*/
BSplineCurve::~BSplineCurve()
{
}

//========================================================
//                   Public Interface
//========================================================

/**
	Gets all of the control points of this curve.
*/
vector<Point3D*>* BSplineCurve::getControlPoints()
{
	return this->_controlPoints;
}

/**
	Determines if the specified point object is the
	one that is currently selected.
*/
bool BSplineCurve::isSelectedPoint(const Point3D* point)
{
	if (!isPointSelected())
	{
		return false;
	}

	return (point == this->_controlPoints->at(this->_selectedIndex));
}

/**
	Specifies whether a point is in the selected state.
*/
bool BSplineCurve::isPointSelected()
{
	return (this->_selectedIndex != -1);
}

/**
    Gets the order of the B-Spline.
*/
int BSplineCurve::getOrder()
{
	return this->_order;
}

/**
    Sets the order of the B-Spline.
*/
void BSplineCurve::setOrder(int order)
{
	this->_order = order;
	updateKnotSequence();
	emit dataDirty();
}

/**
    Gets the sampling increment for points on the 
    B-Spline curve.
*/
double BSplineCurve::getIncrement()
{
	return this->_increment;
}

/**
    Sets the sampling increment for points on the 
    B-Spline curve.
*/
void BSplineCurve::setIncrement(double increment)
{
	printf("new increment %4.4f\n", increment);
	this->_increment = increment;
	this->_majorChange = true;
	emit dataDirty();
}

/**
    Generates a vector of lines to geometrically represent
    the calculation of the curve point at the specified (u)
    parameter value.
*/
BSplineSnapshot* BSplineCurve::getGeometry(double u)
{
	int i; // counter
	int closestIndex;
	double smallestDelta;
	BSplineSnapshot* result = NULL;

	closestIndex = -1;
	smallestDelta = 2.0;
	for (i = 0; i < this->_latestGeometries->size(); i++)
	{
		//printf("getting geometry %4.4f\n", this->_latestGeometries->at(i)->u);
		if (abs(this->_latestGeometries->at(i)->u - u) < abs(smallestDelta))
		{
			closestIndex = i;
			smallestDelta = this->_latestGeometries->at(i)->u - u;
		}
	}

	if (closestIndex != -1)
	{
		result = this->_latestGeometries->at(closestIndex);
	}

	return result;
}

/**
	Calculates and returns all of the points along the
	curve, given the attributes of this class.
*/
vector<Point3D*>* BSplineCurve::getCurvePoints()
{
	double u; // counter
	int    i; // counter
	int    insertIndex;
	int    cutoffIndex;
	double curveInsertValue;
	double curveCutoffValue;

	vector<Point3D*>*         result;
	vector<BSplineSnapshot*>* geometries;
	BSplineSnapshot*          currentGeometry;

	// Return the latest version if nothing changed
	if (!this->_pointMoved  && !this->_majorChange)
	{
		return this->_latestCurve;
	}
	
	if ((int)this->_controlPoints->size() <= this->_order)
	{
		return new vector<Point3D*>();
	}

	
	// Preventing a memory leak of points
	if (this->_latestCurve != NULL)
	{
		for (i = 0; i < (int)this->_latestCurve->size(); i++)
		{
			delete this->_latestCurve->at(i);
		}

		this->_latestCurve->clear();
	}

	if (this->_latestGeometries != NULL)
	{
		for (i = 0; i < (int)this->_latestGeometries->size(); i++)
		{
			delete this->_latestGeometries->at(i);
		}

		this->_latestGeometries->clear();
	}

	// Calculate the new set of curve points
	result = new vector<Point3D*>();
	geometries = new vector<BSplineSnapshot*>();
	for (u = 0.0; u < 1.0; u += this->_increment)
	{
		currentGeometry = new BSplineSnapshot();
		result->push_back(getCurvePoint(u, currentGeometry));	
		geometries->push_back(currentGeometry);
	}

	// Making sure that we connect with the last point
	if (u >= 1.0)
	{
		currentGeometry = new BSplineSnapshot();
		result->push_back(getCurvePoint(1, currentGeometry));
		geometries->push_back(currentGeometry);
	}

	this->_latestGeometries = geometries;
	this->_latestCurve = result;
	this->_majorChange = false;
	this->_pointMoved = false;
	return result;
}

/**
	Adds a control point given the specified reference point.
*/
void BSplineCurve::addControlPoint(Point3D& at)
{
    Point3D* newPoint;

	if (this->_controlPoints != NULL)
	{
        newPoint = new Point3D(at.x, at.y, at.z);
        newPoint->isMidpoint = at.isMidpoint;
        newPoint->vertex = at.vertex;
		this->_controlPoints->push_back(newPoint);
	}

	updateKnotSequence();
	emit dataDirty();
}

/**
	Removes the control point specified by the reference point.
*/
void BSplineCurve::removeControlPoint(Point3D& reference)
{
	int nearestIndex;
	Vector3D* delta;

	if ((this->_controlPoints->size() - 1) < (this->_order + 1))	
	{
		return;
	}

	delta = new Vector3D();
	nearestIndex = findNearestPointIndex(reference, *delta);
	
	if (delta->length() < MAX_DELETE_PROXIMITY)
	{
		this->_controlPoints->erase(this->_controlPoints->begin() + nearestIndex);
		if (nearestIndex == this->_movedPointIndex)
		{
			_movedPointIndex = -1;
		}
	}

	updateKnotSequence();
	emit dataDirty();
}

/**
	Selects the point nearest to the reference point.
*/
bool BSplineCurve::selectControlPoint(Point3D& reference)
{
	int nearestIndex;
	Vector3D* delta;
	
	delta = new Vector3D();
	nearestIndex = findNearestPointIndex(reference, *delta);
	
	if (delta->length() < MAX_SELECT_PROXIMITY)
	{
		this->_selectedIndex = nearestIndex;
		return true;
	}

	return false;
}

void BSplineCurve::movePoint(Point3D* selectedPoint, double x, double y, double z)
{
    selectedPoint->x = x;
	selectedPoint->y = y;
	selectedPoint->z = z;

	this->_pointMoved = true;
	emit dataDirty();
}

/**
	Moves the currently selected point by the specified
	delta vector.
*/
void BSplineCurve::moveSelectedPoint(Vector3D& delta)
{
	Point3D* selectedPoint;

	if (this->_selectedIndex != -1)
	{
		selectedPoint = this->_controlPoints->at(this->_selectedIndex);
		selectedPoint->x += delta.x;
		selectedPoint->y += delta.y;
		selectedPoint->z += delta.z;

		this->_pointMoved = true;
		this->_movedPointIndex = this->_selectedIndex;
		emit dataDirty();
	}
}

/**
	Deselects the currently selected point.
*/
void BSplineCurve::deselectCurrentPoint()
{
	this->_selectedIndex = -1;
}

//========================================================
//                   Helper Methods
//========================================================

/**
    Initializes the attributes of this class to their
    default values, using reasonable assumptions.
*/
void BSplineCurve::initializeAttributes()
{
    // TODO Initialize class attributes to default values
	this->_majorChange = true;
	this->_pointMoved = false;
	this->_movedPointIndex = -1;
	this->_selectedIndex = DEFAULT_SELECT_INDEX;
	this->_order = DEFAULT_ORDER;
	this->_controlPoints = new vector<Point3D*>();
	this->_knotSequence = new vector<double>();
	this->_latestCurve = NULL;
	this->_latestGeometries = NULL;
}

/**
	Calculates the index of the point nearest to the specified 
	reference point.
*/
int BSplineCurve::findNearestPointIndex(const Point3D& reference, Vector3D& smallestDelta)
{
	int nearestIndex;
	int i; // counter
	Point3D* currentPoint;

	nearestIndex = -1;
	smallestDelta.x = 1000;
	smallestDelta.y = 1000;
	smallestDelta.z = 1000;
	for (i = 0; i < (int)this->_controlPoints->size(); i++)
	{
		currentPoint = this->_controlPoints->at(i);
		if (abs(currentPoint->x - reference.x) < abs(smallestDelta.x)  ||
			abs(currentPoint->y - reference.y) < abs(smallestDelta.y)  ||
			abs(currentPoint->z - reference.z) < abs(smallestDelta.z))
		{
			nearestIndex = i;
			smallestDelta.x = currentPoint->x  - reference.x;
			smallestDelta.y = currentPoint->y - reference.y;
			smallestDelta.z = currentPoint->z - reference.z;
		}
	}

	return nearestIndex;
}

void BSplineCurve::updateKnotSequence()
{
	int i; // counter	
	int numSegments;
	double knotIncrement;

	this->_knotSequence->clear(); 

	// No point in continuing because the curve
	// will be trivial
	if ((int)this->_controlPoints->size() <= this->_order)
	{
		return;
	}

	// Curve-start multiplicity
	for (i = 0; i < this->_order; i++)
	{
		this->_knotSequence->push_back(0);
	}

	// Recalculate the knot sequence values
	numSegments = (this->_controlPoints->size() - this->_order + 1);
	knotIncrement = 1.0 / (double)numSegments;
	for (i = 1; i < numSegments; i++)
	{
		this->_knotSequence->push_back(knotIncrement * i);
	}

	// Curve-end multiplicity
	for (i = 0; i < this->_order; i++)
	{
		this->_knotSequence->push_back(1);
	}

	this->_majorChange = true;
}

/**
    Calculates and returns the point on the B-Spline curve
    given the attributes of this class and the specified (u)
    parameter. An optional parameter is used to store a
	geometric representation of the calculation.
*/
Point3D* BSplineCurve::getCurvePoint(double u, BSplineSnapshot* geometry)
{
	int      delta;    // index of closest leftmost knot
	int      i, r, s;  // counters
	int      g;        // geometry counter
	double   omega;    // deBeer coefficient
	Point3D* result;
	Line3D*  lastGeomLine;

	vector<Point3D> intermediates; // intermediate deBeer points    
	vector<Point3D> geometryPoints; // will be interpreted as lines

	delta = this->getDelta(u);
	for (i = 0; i < this->_order; i++)
	{
		intermediates.push_back(*(this->_controlPoints->at(delta - i)));
	}

	if (geometry != NULL)
	{
		//printf("calculating geometries for %4.4f\n", u);
		geometry->setU(u);
	}

	for (r = this->_order; r >= 2; r--)
	{
		i = delta;
		for (s = 0; s <= (r - 2); s++)
		{
			omega = (u - this->_knotSequence->at(i))/(this->_knotSequence->at(i+r-1) - this->_knotSequence->at(i));
			intermediates[s] = intermediates[s] * omega + intermediates[s + 1] * (1 - omega);
			i--;

			// Store the geometry points
			if (geometry != NULL)
			{
				geometryPoints.push_back(intermediates[s]);
			}
		}
		
		// Interpret geometry as lines
		if (geometry != NULL)
		{
			geometry->initFromPoints(geometryPoints);
			geometryPoints.clear();
		}
	}

	result = new Point3D(intermediates[0].x, intermediates[0].y, intermediates[0].z);
	intermediates.clear();

	return result;
}

/**
    Calculates the index of the nearest left-most knot, in
    parameter space, given the parameter u. The knot sequence 
    is sorted, so we can do a binary search.
*/
int BSplineCurve::getDelta(double u)
{
    const int SEARCH_FAILED = -1;
    
    int startIndex;
    int middleIndex;
    int endIndex;
    double middleValue;
    double nextValue;
    
    // Can't do it if we don't have a knot sequence
    if (this->_knotSequence == NULL)
    {
        return SEARCH_FAILED;
    }
    
	startIndex = 0;
    endIndex = this->_knotSequence->size() - 1;

	// Prevents an infinite loop
	if (this->_knotSequence->at(startIndex) == u)
	{
		return this->_order - 1;
	}
	else if (this->_knotSequence->at(endIndex) == u)
	{
		return endIndex - this->_order;
	}

    while (startIndex <= endIndex)
    {
        // We'll need the middle index
        middleIndex = (int)((startIndex + endIndex) / 2);
        
        // In case there's only one element left to search
        if (middleIndex == endIndex)
        {
            // We then know we are returning the closest
            // delta, because after this the search is over
            return middleIndex;
        }
        
        // Search the middle element
		middleValue = this->_knotSequence->at(middleIndex);
        nextValue = this->_knotSequence->at(middleIndex + 1);
        if (middleValue <= u  &&  nextValue > u)
        {
            return middleIndex;
        }
        // Search the lower half
        else if (u < middleValue)
        {
            if (middleIndex == startIndex)
            {
                // This means there were two elements and
                // delta could not be found between them. Hence
                // delta does not exist in this knot sequence
                return SEARCH_FAILED;
            }
            
            endIndex = middleIndex;
        }
        // Search the upper half
        else if (u >= nextValue)
        {
            startIndex = middleIndex + 1;
        }
    }
    
    return SEARCH_FAILED;
    
}


